// --------------------------------------------------------------------------------------------------------------------
// <copyright file="RobotiqSModelGripper.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Robotiq
{
    using System;
    using System.Net.Sockets;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Hardware.Modbus;
    using Microsoft.Robotics.Hardware.Robotiq;

    /// <summary>
    /// Manages state and interface to a Robotiq S Model Gripper
    /// Manual for Robotiq S Model:  <a href="http://support.robotiq.com/pages/viewpage.action?pageId=592020" />
    /// </summary>
    public class RobotiqSModelGripper : IDisposable
    {
        /// <summary>
        /// Default IP address for gripper
        /// </summary>
        private const string DefaultGripperIP = "192.168.1.11";

        /// <summary>
        /// Default Unit ID
        /// </summary>
        private const byte DefaultUnitId = 0x02;

        /// <summary>
        /// Address of the first input register
        /// </summary>
        private const ushort AddressOfFirstInputRegister = 0x0000;

        /// <summary>
        /// Word count of input registers
        /// </summary>
        private const byte InputRegisterWordCount = 0x08;

        /// <summary>
        /// Approximate update cycle of gripper in ms.
        /// </summary>
        private const int GripperServoCycleInMs = 10;

        /// <summary>
        /// Unit ID
        /// </summary>
        private byte unitID;

        /// <summary>
        /// Modbus connection to the gripper
        /// </summary>
        private ISimpleModbusTCPConnection modbusConnection;

        /// <summary>
        /// Tracks if we have been disposed or not
        /// </summary>
        private bool disposed = false;

        /// <summary>
        /// Initializes a new instance of the <see cref="RobotiqSModelGripper" /> class.
        /// </summary>
        public RobotiqSModelGripper()
        {
            this.InitializeState();

            this.ConnectToGripper(DefaultGripperIP);
            this.unitID = DefaultUnitId;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="RobotiqSModelGripper" /> class.
        /// </summary>
        /// <param name="modbusTCP">ModbusTCP connection to use.</param>
        public RobotiqSModelGripper(ISimpleModbusTCPConnection modbusTCP)
        {
            this.InitializeState();

            this.unitID = DefaultUnitId;
            this.modbusConnection = modbusTCP;
        }
 
        /// <summary>
        /// Gets or sets state of the gripper
        /// </summary>
        public RobotiqSModelState State { get; set; }

        /// <summary>
        /// Get the grip mode.
        /// </summary>
        /// <param name="register">Register 0 returned by the gripper.</param>
        /// <returns>The grip mode.</returns>
        public static RobotiqSModelGripModes GetGripMode(byte register)
        {
            if ((register & RobotiqSModelValues.GripModeMask) == RobotiqSModelValues.PinchMode)
            {
                return RobotiqSModelGripModes.Pinch;
            }
            else if ((register & RobotiqSModelValues.GripModeMask) == RobotiqSModelValues.WideMode)
            {
                return RobotiqSModelGripModes.Wide;
            }
            else if ((register & RobotiqSModelValues.GripModeMask) == RobotiqSModelValues.ScissorMode)
            {
                return RobotiqSModelGripModes.Scissor;
            }
            else
            {
                return RobotiqSModelGripModes.Basic;
            }
        }

        /// <summary>
        /// Gets the gripper status
        /// </summary>
        /// <param name="register">Register 0 returned by the gripper.</param>
        /// <returns>Gripper status</returns>
        public static RobotiqSModelStatus GetGripperStatus(byte register)
        {
            if ((register & RobotiqSModelValues.GripperStatusMask) == RobotiqSModelValues.ActivationInProgress)
            {
                return RobotiqSModelStatus.ActivationInProgress;
            }
            else if ((register & RobotiqSModelValues.GripperStatusMask) == RobotiqSModelValues.ModeChangeInProgress)
            {
                return RobotiqSModelStatus.ModeChangeIsInProgress;
            }
            else if ((register & RobotiqSModelValues.GripperStatusMask) == RobotiqSModelValues.ActivationAndModeChangeComplete)
            {
                return RobotiqSModelStatus.ActivationAndModeChangeComplete;
            }
            else
            {
                return RobotiqSModelStatus.Reset;
            }
        }

        /// <summary>
        /// Gets the grip status
        /// </summary>
        /// <param name="register">Register 0 returned by the gripper.</param>
        /// <returns>The grip status</returns>
        public static RobotiqSModelGripStatus GetGripStatus(byte register)
        {
            if ((register & RobotiqSModelValues.GripStatusMask) == RobotiqSModelValues.OneOrTwoFingersStoppedBeforeReference)
            {
                return RobotiqSModelGripStatus.OneOrTwoFingersStoppedBeforeRequestedPosition;
            }
            else if ((register & RobotiqSModelValues.GripStatusMask) == RobotiqSModelValues.AllFingersStoppedBeforeReference)
            {
                return RobotiqSModelGripStatus.AllFingersStoppedBeforeRequestedPosition;
            }
            else if ((register & RobotiqSModelValues.GripStatusMask) == RobotiqSModelValues.AllFingersReachedReference)
            {
                return RobotiqSModelGripStatus.Stopped;
            }
            else
            {
                return RobotiqSModelGripStatus.InMotion;
            }
        }

        /// <summary>
        /// Set the status of a single finger (or scissor axis)
        /// </summary>
        /// <param name="register">Register containing status information, shifted so that bits
        ///  for finger in question are in the two LS bits.</param>
        /// <returns>Finger status</returns>
        public static RobotiqSModelFingerStatus GetFingerStatus(byte register)
        {
            if ((register & RobotiqSModelValues.FingerStatusMask) == RobotiqSModelValues.FingerHasStoppedDueToContactWhileOpening)
            {
                return RobotiqSModelFingerStatus.StoppedDueToContactWhileOpening;
            }
            else if ((register & RobotiqSModelValues.FingerStatusMask) == RobotiqSModelValues.FingerHasStoppedDueToContactWhileClosing)
            {
                return RobotiqSModelFingerStatus.StoppedDueToContactWhileClosing;
            }
            else if ((register & RobotiqSModelValues.FingerStatusMask) == RobotiqSModelValues.FingerIsAtRequestedPosition)
            {
                return RobotiqSModelFingerStatus.AtRequestedPosition;
            }
            else
            {
                return RobotiqSModelFingerStatus.InMotion;
            }
        }

        /// <summary>
        /// Dispose of unmanaged resources
        /// </summary>
        public void Dispose()
        {
            // follow standard dispose pattern
            this.Dispose(true);

            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Resets the gripper to active or inactive state.
        /// Sets to "basic" grip mode, and go to requested position bit off.
        /// Throws an exception on timeout.
        /// </summary>
        /// <param name="activate">If true activate the gripper, otherwise deactivate it.</param>
        /// <param name="timeoutInMs">Timeout and throw after this many ms.</param>
        public void ResetGripper(bool activate, int timeoutInMs)
        {
            // check that we're either in 'reset' (unactived or activation required) state OR
            // that all activations and mode changes are complete
            DateTime startTime = DateTime.Now;
            do
            {
                if (DateTime.Now.Ticks - startTime.Ticks > TimeSpan.TicksPerMillisecond * timeoutInMs)
                {
                    throw new TimeoutException();
                }

                System.Threading.Thread.Sleep(GripperServoCycleInMs);
                this.UpdateState();
            }
            while (this.State.DeviceStatus != RobotiqSModelStatus.Reset
                && this.State.DeviceStatus != RobotiqSModelStatus.ActivationAndModeChangeComplete);

            if (this.State.IsActivated == activate)
            {
                return;
            }

            // now send an activation packet
            // see Robotiq S Model manual for details
            byte[] robotiqActivationRegisters = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };

            if (activate)
            {
                robotiqActivationRegisters[0] = 0x01;
            }

            ushort[] modbusRegisters = this.RobotiqRegistersToModbusRegisters(
                robotiqActivationRegisters);

            this.modbusConnection.PresetMultipleRegisters(
                DefaultUnitId,
                0x0000,
                (ushort)modbusRegisters.Length,
                modbusRegisters);
        }

        /// <summary>
        /// Set the command registers of the gripper based on a 'basic' command
        /// </summary>
        /// <param name="command">Command to set.</param>
        public void SetBasicCommand(RobotiqSModelBasicCommand command)
        {
            // now send an activation packet
            // see Robotiq S Model manual for details
            byte[] basicCommandRegisters = new byte[6];

            // register 0 is action request
            basicCommandRegisters[0] = this.SetRegister0(command.Activate, command.GripMode, command.GoToPosition);

            // register 1 no used in basic mode
            basicCommandRegisters[1] = 0x00;

            // register 2 is reserved
            basicCommandRegisters[2] = 0x00;

            // register 3 is position
            basicCommandRegisters[3] = command.Position;

            // register 4 is speed
            basicCommandRegisters[4] = command.Speed;

            // register 5 is force
            basicCommandRegisters[5] = command.Force;

            ushort[] modbusRegisters = this.RobotiqRegistersToModbusRegisters(
                basicCommandRegisters);

            this.modbusConnection.PresetMultipleRegisters(
                DefaultUnitId,
                0x0000,
                (ushort)modbusRegisters.Length,
                modbusRegisters);
        }

        /// <summary>
        /// Set Robotiq S model register 0
        /// </summary>
        /// <param name="activate">Activation command</param>
        /// <param name="mode">Grip mode command</param>
        /// <param name="gtp">Go to position command</param>
        /// <returns>Corresponding register 0 byte</returns>
        public byte SetRegister0(bool activate, RobotiqSModelGripModes mode, bool gtp)
        {
            byte register = 0x00;

            if (activate)
            {
                register |= RobotiqSModelValues.GripperActivated;
            }

            switch (mode)
            {
                case RobotiqSModelGripModes.Pinch:
                    register |= RobotiqSModelValues.PinchMode;
                    break;
                case RobotiqSModelGripModes.Scissor:
                    register |= RobotiqSModelValues.ScissorMode;
                    break;
                case RobotiqSModelGripModes.Wide:
                    register |= RobotiqSModelValues.WideMode;
                    break;
            }

            if (gtp)
            {
                register |= RobotiqSModelValues.GoToPositionActive;
            }

            return register;
        }

        /// <summary>
        /// Updates the state of the gripper.
        /// </summary>
        public void UpdateState()
        {
            ushort[] modbusRegisters;

            // query the gripper for it's output registers over Modbus
            this.modbusConnection.ReadInputRegisters(
                this.unitID,
                AddressOfFirstInputRegister,
                InputRegisterWordCount,
                out modbusRegisters);

            byte[] robotiqRegisters = ModbusRegistersToRobotiqRegisters(modbusRegisters);

            // Use the contents of the registers to populate the gripper state.
            // Bit 0 of register 0 indicates activation status
            if ((robotiqRegisters[0] & RobotiqSModelValues.ActivationMask) == RobotiqSModelValues.GripperActivated)
            {
                this.State.IsActivated = true;
            }
            else
            {
                this.State.IsActivated = false;
            }

            // Bits 1 and 2 of register 0 indicate grip mode
            this.State.GripMode = RobotiqSModelGripper.GetGripMode(robotiqRegisters[0]);
            
            // Bit 3 of register 0 indicates "to go bit" status
            if ((robotiqRegisters[0] & RobotiqSModelValues.GoToPositionMask) == RobotiqSModelValues.GoToPositionActive)
            {
                this.State.GoToPosition = true;
            }
            else
            {
                this.State.GoToPosition = false;
            }

            // Bits 4 and 5 of register 0 indicate the gripper status
            this.State.DeviceStatus = RobotiqSModelGripper.GetGripperStatus(robotiqRegisters[0]);

            // Bits 6 and 7 of register 0 indicate the motion status
            this.State.GripStatus = RobotiqSModelGripper.GetGripStatus(robotiqRegisters[0]);

            // Register 1 contains 2 bits for the motion status of each finger and the scissor axis
            this.State.FingerAState.FingerStatus = RobotiqSModelGripper.GetFingerStatus(robotiqRegisters[1]);
            this.State.FingerBState.FingerStatus = RobotiqSModelGripper.GetFingerStatus((byte)(robotiqRegisters[1] >> 2));
            this.State.FingerCState.FingerStatus = RobotiqSModelGripper.GetFingerStatus((byte)(robotiqRegisters[1] >> 4));
            this.State.ScissorState.FingerStatus = RobotiqSModelGripper.GetFingerStatus((byte)(robotiqRegisters[1] >> 6));

            // First 4 bits of register 2 contain the current fault code (second 4 bits reserved)
            this.State.Fault = (RobotiqSModelFaults)(robotiqRegisters[2] & 0x0F);

            // Set all commanded positions to the value in register 3. 
            // Assumes basic operation mode where commanded positions are the
            // same for all fingers.
            this.SetAllCommandedPositions(robotiqRegisters[3]);

            // register 4 contains finger A actual position
            this.State.FingerAState.Position = robotiqRegisters[4];

            // register 5 contains finger A current in 0.1*mA
            this.State.FingerAState.CurrentInMilliAmps = robotiqRegisters[5] * 10.0;

            // register 7 contains finger B actual position
            this.State.FingerBState.Position = robotiqRegisters[7];

            // register 8 contains finger B current in 0.1*mA
            this.State.FingerBState.CurrentInMilliAmps = robotiqRegisters[8] * 10.0;

            // register 10 contains finger C actual position
            this.State.FingerCState.Position = robotiqRegisters[10];

            // register 11 contains finger C current in 0.1*mA
            this.State.FingerCState.CurrentInMilliAmps = robotiqRegisters[11] * 10.0;

            // register 13 contains scissor axis actual position
            this.State.ScissorState.Position = robotiqRegisters[13];

            // register 14 contains scissor axis current in 0.1*mA
            this.State.ScissorState.CurrentInMilliAmps = robotiqRegisters[14] * 10.0;
        }

        /// <summary>
        /// Generate 8 bit 'Robotiq' registers from  16 bit Modbus registers
        /// </summary>
        /// <param name="modbusRegisters">16-bit registers read from the modbus interface.</param>
        /// <returns>Corresponding 8-bit registers corresponding to gripper registers.</returns>
        internal static byte[] ModbusRegistersToRobotiqRegisters(ushort[] modbusRegisters)
        {
            int numRobotiqRegisters = modbusRegisters.Length * 2;

            byte[] robotiqRegisters = new byte[numRobotiqRegisters];

            for (int i = 0; i < modbusRegisters.Length; ++i)
            {
                robotiqRegisters[i * 2] = (byte)(modbusRegisters[i] & ModbusTCPPacket.LowestByteMask);
                robotiqRegisters[(i * 2) + 1] = (byte)((modbusRegisters[i] >> 8) & ModbusTCPPacket.LowestByteMask);
            }

            return robotiqRegisters;
        }

        /// <summary>
        /// Dispose of unmanaged resources.
        /// </summary>
        /// <param name="disposing">True if called by a user's code.</param>
        protected virtual void Dispose(bool disposing)
        {
            if (!this.disposed)
            {
                if (disposing)
                {
                    this.modbusConnection.Dispose();
                }
            }
        }

        /// <summary>
        /// Initialize state variables
        /// </summary>
        private void InitializeState()
        {
            this.State = new RobotiqSModelState();
            this.State.FingerAState = new RobotiqSModelFingerState();
            this.State.FingerBState = new RobotiqSModelFingerState();
            this.State.FingerCState = new RobotiqSModelFingerState();
            this.State.ScissorState = new RobotiqSModelFingerState();
        }

        /// <summary>
        /// Sets requested position for all fingers to a single value.  Used in "basic" operation mode
        /// </summary>
        /// <param name="register">Position value.</param>
        private void SetAllCommandedPositions(byte register)
        {
            this.State.FingerAState.CommandedPosition = register;
            this.State.FingerBState.CommandedPosition = register;
            this.State.FingerCState.CommandedPosition = register;
            this.State.ScissorState.CommandedPosition = register;
        }

        /// <summary>
        /// Open a ModbusTCP connection to the gripper
        /// </summary>
        /// <param name="ipAddress">IP address of the gripper.</param>
        private void ConnectToGripper(string ipAddress)
        {
            this.modbusConnection = new SimpleModbusTCPConnection(ipAddress);
        }

        /// <summary>
        /// Generate 8 bit 'Robotiq' registers from  16 bit Modbus registers
        /// </summary>
        /// <param name="robotiqRegisters">8-bit robotiq registers</param>
        /// <returns>Corresponding 16-bit registers corresponding to ModbusTCP registers.</returns>
        private ushort[] RobotiqRegistersToModbusRegisters(byte[] robotiqRegisters)
        {
            int numModbusRegisters = robotiqRegisters.Length / 2;

            ushort[] modbusRegisters = new ushort[numModbusRegisters];

            for (int i = 0; i < numModbusRegisters; ++i)
            {
                modbusRegisters[i] = (ushort)(robotiqRegisters[i * 2] + (ushort)((ushort)robotiqRegisters[(i * 2) + 1] << 8));
            }

            return modbusRegisters;
        }
    }
}
